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VARIATIONAL AI.CUKJ IIMS I- OR NONLI NKAR SMOOTH! NO APPLICATIONS 

Ralph K. HiU'li, Jr.* 

Alims Research Center 

SUMMARY 

This report presents ;i variational approach for solving a nonlinear, 
fixed- interval smoothing problem with application to offline processing of 
noisy data for trajectory reconstruction and parameter estimation. The non- 
linear problem is solved as a sequence of linear two-point boundary-value 
problems (TPBVP) . Second-order convergence properties are demonstrated. 
Algorithms for both continuous and discrete versions of the problem are given, 
and example solutions are provided. 


1 . INTRODUCTION 

Smoothing applications generally involve offline processing of noisy data 
records for trajectory reconstruction and parameter estimation. The fixed- 
interval smoothing problem is conveniently formulated as one of minimising a 
suitable performance measure subject to dynamic constraints. This formulation 
is equivalent to a Bolza problem in the calculus of variations (ref. 1). 

Bryson and Frazier (ref. 2) first gave a solution for the linear, continuous 
rase, in which a "sweep" method was used to solve the resulting two-point 
boundary-value problem (TPBVP). Cox (ref. 3) later formulated and solved a 
linear, discrete problem in a similar way. 

Solution of the nonlinear smoothing problem requires an iterative pro- 
cedure that converges and is computationally feasible. Many techniques have 
been proposed (ref. A), but only a few have been applied in practice. In one 
approach, an approximate sweep method is used to solve the nonlinear TPBVP of 

*R . K . Bach, Jr., Is with the Department of F.leoirical Engineering, 
NortheasLcrn University, Boston, Mass. 02115. He is a visiting Research 
Scientist at Ames Research Center under an Intergovernmental Personnel 
Agreement . 
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the Bo 1 za for mill at ion. Tin? resulting a I j;oi i I Inn consists of an extended 
Kalman filter-smoother. A typical imp I omen t at Ion Is described in rol erence r >. 
While this algorithm Is common Ly used, its convergent < properties are dLlli- 
c.ult to predict because linearisation is about a filtered trajectory. 

A second-order variational procedure is applied here to the nonlinear 
smoothing problem. This approach leads to an algorithm that combines a 
Newton-Raphson computation of parameter changes with a stable information 
filter-smoother to improve the state estimates at each iteration. Lineariza- 
tion is about a smoothed trajectory, and quadratic convergence to a minimum of 
the performance measure is demonstrated. In the absence of process noise, the 
algorithm is shown to be mathematically equivalent to other Newton-Raphson 
methods used for parameter estimation (refs. 6 and 7). 

The basic analytical approach used to derive the algorithm is not new: 
it is an example of the "successive sweep" method of McReynolds and Bryson 
(ref. 8), originally devised to solve a continuous optimal, control problem. 
Furthermore, Sage and Melsa (ref. 9) outlined the application of this method 
in solving the discrete smoothing problem. The development presented here 
extends and unifies previous work, and also directs attention to an apparently 
neglected but very useful tool for state and parameter estimation. 

II. STATEMENT OK PROBLEM 


The continuous version of the smoothing problem can lie stated as follows: 
Given a system of the form 

x = f(x,w) , x(t Q ) = x 0 (2.1) 

with a continuous measurement 


■/. ~ h(x) + v 


( 2.21 


available over the interval (t 0 ,t ( ), determine x {) , x = x(t), and w = w(l) 
so that a performance measure 


J = 


o/2)(x„ - ;„)V<x„ 


v 


+ d/2) 


f 


(w^Q“ *w 


+ 


T 

v l R“ 


Mdt (2.3) 
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/ 


is minimized. !u lliis fuiiiiiil.il im, .in ,i priori esl im.il.c of t fn* stale 

‘It t ” L () , ;iml l’ o , <), and K .i,.- u. i ;■ i i c in), i';.. * l tiic;,. 11,,- vector w wit ) 
‘‘■in be considered .‘in "unknown* input . i- uiion modeled ns process noise, 

while v = v(t) Is Miu "error" in I Im r ■ ,.uii nt. . 


file discrete version oi : i . ■ ■ :i= • Imim; p- -1 • I ■ -m is Im ■•<> 1 1 . > i i-d in .i simil.i: 

manner: (liven a system <•! tie !nn;i 

x(i + I ) i | :■;(. i ) , w( i ) 1 , x(0) - x (> (2.4) 

with a measurement sequence 

2(0 = h [ x ( i ) ] I- v ( i ) , i = l, 2, ..., N (2.5) 

determine x Q and sequences [x(0 1 and [w(i) | so that a performance measure 

T N " 1 T ... 

J = (l/2)(x 0 -x 0 ) p ~ 1 (x o -x 0 ) i v I / ;• ) £ |w (i)Q 1 w(i) + v'd+DR-'vO + Dl 

( 2.61 

is minimized. 


The reader can consult reference 9 lor a Bayesian maxi mum- likelihood 
interpretation of criteria (2.1) and (2.6). For this discussion, a weighted 
least-squares interpretation would also be appropriate. The continuous and 
discrete versions of the smoothing problem are each examples of a Bolza 
problem, which are solved here by use ol a second-order variational procedure. 
Note that the smoothing I ornuilat ton permits the inclusion of constant param- 
eters as state variables. Thus, parameter identification is a special case of 
state-variable estimation. 


111. CONTI Nc(U;S Al.UOKi.THM 

To develop an algorithm lor solving Mu- continuous smoothing problem, 
first adjoin the dynamic constraint (2.1) in the performance measure (2.3) 
with Lagrange multiplier A *= .v(t) to obtain 

'x)dt O.l) 
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The necessary conditions lor a minimum .] art determined In the usual way by 
requiring that the first variation vanish (ref. I). These conditions are 
given by (2.1) and 


J ■ -*„ T , 


0 = K, 


A(t f ) = 0 


A < fc o> ^ "On 


(3.4) 

(3.5) 


where the letter subscript indicates a partial derivative operation. Equa- 
tions (2.1), (3.4), and (3.5) define a nonlinear TPBVP. It is simple enough 
to choose an x Q , w(t) and to generate nominal trajectories x(t), A(t) by 
solving (2.1) and (3.4). Such trajectories, however, are not likely to yield 
a minimum performance measure (neither condition of (3.5) is satisfied). 

The algorithm is developed by considering the effect on the augmented 
performance measure caused by changes 4x(t), Aw(t), and <U(t) from nominal 
trajectories. Equation (3.1) is expanded to second order to obtain: 
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0**0 + J 3( w* 


w dt + ( j / 2 ) 5 x o J (*x X ) o 0 x o 
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The objective now is to determine .'x (> , nx(t), and .\ w (l) so that 4J is 
as large a negative number as possible at each iteration. Convergence is 
attained when J reaches a minimum. in this "accessory minimization" problem, 
<5A - <$A(t) act; as a Lagrange multiplier for a dynamic constraint, equivalent 
to a first-order expansion of (2.1). The necessary conditions for minimizing 
6J in (3.6) are given by 
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Substituting (3.L0) into 0.7) ,md 0.8) yields the Li. .ear, nonhu m o,.,,u>ous 
TI’BVP : 


~ '■ x “ 1 Ai 1 w ~ f„^( w T , fix, /unknown) 0.1 I) 


lU " " (3f xx - “xwO^* - <*X - + . 


<$X(t f ) = 0 . P.J.’l 

Any one of a number of "sweep'' solutions may be used for a linear TPl)\i\ 
Here It is convenient to introduce M * M(t) and m = nut) so rli.it 


6 A = M6x + m 


H(t f ) = 0 ; m(t .) = () . 


(3.13; 

Now differentiate (3.13) and use (3.1 1) ;m J (3.12) to obtain the differential 
equations : 
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Equations (3.14) and (3.15) const i l mr a "backward information filter," i.o., 
their solutions are pronagu* cl hacM-.-.iid itnai t -- tj until 1 = t , The 
change in x 0 needed to in it i.i l ui- the forward smoothing pass is determined 
by substituting (3.13) into (3 . ‘M and solving i or Ax : 
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Now 

, differentiate 

(3. 

30) and u.si 

■ 13.28) 

and 

(3 

.29) to 

obtain 

the 

different ia 1 


equations : 


M , _r x M - Ml - !* x ' i' 1 l.jj + Mf w Qt w M 
>* ” - 1 v/J 1 w ' M) ’ I' Mf,.,w + 1 R“ 1 \ 
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(3.3]) 

(3.32) 


which constitute the backward ini onu.it (an I f Iter. The forward information 
smoother, determined by suhst i l m inj-. (3.10) into (3.28), i given hv 


A* - ( 1 x - ) Qf M )•■■■>; - ‘ <V f t 1 1 ,) 

a W \ . V; • V/ ' 


(3. 33) 


where 6x () , as determined using (’..'3) and < ). 30) in (3.9), is the same as 
(3.17). The change in the unknown lordnj 1 unol ion is I mind from (3.10) to bo 


i \: 


w • til ( t f- M> v) , 


(3. J'.) 


The steps in the nl gor Lth:n ate sumitiai i /mi he low. 
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measure from J). 
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(4) Update; x u and w(l ) oiul Iterate mil i 1 Ax and '•■w(t) ;irr "‘ini I i- 

ciently” snuil L and .1 is mi n i i.ii xml . 


Some comments concert] i n.r 1 In- algm i i Inn prcsenlfd lure are in order. 
First, (3.31) to (3.34) are t ecnv.n i xml as tin usual backward informal ion fil- 
ter, forward information smoother solution of a linear smoothing problem. In 
fact, any one of the classical so! us ions (n is. 4 and 1.0) may h implemented 
for the linear TPBVP of (3.28) and (3,29). 1 The important point here is that 
the nonlinear smoothing problem can he solved as a converging .sequence of 
linear TPBVP solutions. Also, note that the approximations of (3.2'.) a f I'm I 
only the radius of convergence: win n convergence is attained, iSx(t), ; w(t), 

and 6A(t) vanish and u ~ e - ', so lliat. ti.32), (3.34), and (3.9) reduce to 
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A - - 1 x > i- lt x U : v , 


Ut f ) = 0 
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■'(•J - Y. (x -> ~ ■ 


(3.33) 

(3.36) 


These equations are equivalent to ( !.V) and (). r >), the conditions necessary 
for a mini mum performance measure. 


Finally, note that, il there i ■; no process noise assoc i. it ed with the 
model, the equations lor M(t ) ami i(t) are much simpler. Storage require- 
ments are reduced since the lorw.tid smoother equations need not he solved. 
The algorithm becomes a second ord< t procedure for parameter Identifies! ion, 
which is equivalent to but easier to implement than the modi I' fed Newtou- 

1 Refer to appendix (I loi another (equivalent) a Ip.or it hi: for so tut ion o' 

the continuous smoothing, problem. 
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(A. id) 

■: .’i . ! 

(•i.l . ) 
(A. t‘>) 

’ . Cm H>) 


1 





1 


Now, sol 

v<* (4. 14) and (4. 1 ' 

>) harfwal 

d , 1 ruin i 

- N - 1 to i - 0. To 

i n i 1 

ial 

1 V.e t ll>' 

i oi ward suu ml h i up. i 

• a , :.uh: 

.1 i i ui ■■ (4, 

13) Into (4.0) and so 1 vo 

1 or 



oh la 1 n 







'' ,x o 

f !•' 1 f M 
o , 

1 [ l‘ • ( X 

i 1 «i n 

. “ \,> H ' % 1 

(4. 

10 

win 1 re 


I 1 1 

1) t> 

v : 

(,f, xx>o = ’’o' 

(4. 

HO 


"o 

=~ •>(<>) t- 

in(O) ; 

M o - M (0) 

(4, 

.19) 


;irc used. The expression lor Lhe change ot initial condition is seen to he a 
Newton- Kaphsoti i:a I eu1.nti.on. To obtain the change in the forcing function, 
so L ve (4.7) i'o rwa rd , w i t h 

Aw(i) - -qlfJmU h- 1) + ifj + i, T 6x (i) ] . (4.20J 

The algorithm can he applied iteratively until the performance measure 
(2.6) reaches a minimum. Convergence properties can be demonstrated by observ- 
ing that 6J in (4.6) can be expressed as a sum of quadratic forms. The 
derivation is facilitated by adding the zero value 

N- 1 

-0/2) £ '“'xOi + UM(i + l)U x ox(i) + f w 4w(i) - 6x(i 4- 1)] 

[=ii 

to (4.6) and substituting for i\ 1 ( i + 1) 1 rom (4.13). The expression for S.l 
eventually reduces to 


.0 = l (x - x ) 1 + a ' ] 6x T (l/2)6x ^(H~‘ + M )6x 
o a o o c> ' o o o o 

N- 1 

-0/2) D fin'd t ! ) I w I ll w lg|t M l m(i + 1) H tt/] . (4.2!) 

•=() 

Note’ that tiie gradient of .1 with respect to x appears in tlu: first term 
of (4.21); the liessian (,i\i/.)x ' ) is lomxl in the second ti’rm. Now, suhstl- 
L ui e t lie minimizing Ax, from (4,17) and obtain 


(l/2)4x o l (l'“ 1 -l M o )5x o 0/2) J)|in <i + 1 ) I w + 3f l# )<>[ f„ m( i + I.) t | 

(4.22) 


i () 
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(, o + H tl ) • 0 ; Q . 0 . (/,.„ 


l’r;u i_ i cal Imp J t mental ion 

The radius of convergence can be effectively increased and the compula- 
tions simplified by again eliminating all second-partial derivatives from 
3f xx , -If^, and In the discrete case, 

,<xs * f xV R "'Vx • ,( xv, * f x T l'« T »' 1 hxf» i 


,f w a f,\Vv, + Q ' 1 • 


To Obtain a simplified linear TPBVP , introduce the variable 


i-(i) = A ( 1 ) + 6 A Cl) 


and use (4.24), (4.25) and 


-x T “ f x r f >(i + !) ~ h x T R _1 v(i + 1)] 

T T t 

■ 7f w ' f w l' (i + D ■ \ R -i v(i + 1)1 + Q -1 w(i) 


i» (4.7) and (4.8). The result is 

ix(t + 1 ) - f x s*u) - f„Qr„ T t ; T „(i) - r u w(i) , .ix( 0 ) . «* t 

p ( l> - f x T jp(l + 1) - l. x V'|v!i + 1) - h x *x(J + 1)]J , 

A sweep solution of (a. 28) and (4.29) with 


P(N) - 0 . (4.29) 


P(i) - M(i)ix(i ) + u(i) ; M(N) - 0 ; a(N) = 0 (4.30) 

ylplxlp the backward Inform.,, I„„ m t «r/ r, presented by n manaurnmont npdnt,., 

f>U f 1) - n(l + 1) - b^R-'vd + I) ; t 

p/'t 4. uf! j . , IV- 1. r (4.31) 


3(i + D = a(i + 1) - l' x 1 R~ 1 v(i 4 1) ; 


P(i + 1) , M ( i 4 1) 4 hjR-1}, , 

A X 


the hi! L ° aP, T' ( U!iX D l0r ‘ t,,nn "' r (^mivnlunt) algorithm for solution of 
tin disc rete smoothing problem. 


and a time up t, ate : 


6 - iq" 1 + r w 'i'u i Ui w l * ; (‘(i) =- r> r w 1 1 * Cl * i) (''*• t'O 

t(0 - l v '[l - l 1 I.'Ui + I) - l’(i f 1 > r w w ( i ) ) ; I 

M(i) - r x r ! r - f w c:n)i'i*(i i i)i N . I 

The change 6x (> required lu initialize the forward smoother (4.7) is 
again given by (4.17), whiJe the change in forcing function can be written: 

6w(i) = ~[1 - C(i)f w ][w(i) + Qf w ‘V*(i + 1)1 - C(i)f x 4x(i) . (4.34) 

The steps in the discrete algorithm are summarized below. 

(1) Use x Q , [w(i)J obtained from the preceding iteration (or an initial 
guess) to compute a smoothed trajectory fx(i)] from (2.4) and the performance 
measure from (2.6). 

(2) Solve the "backward information filter" (4.31) and (4.33) to obtain 
[M(i)l and f a (i) J ; store elements necessary for the next step. 

(3) Perform the Newton- Raphson computation (4.17) for 6x n and solve 
the "forward smoother" (4.7) and (4,30 to determine [4w(i)]. 

(4) Update x Q and fw(i) | and iterate until <ix 0 , [6w(i)] are "suffi- 
ciently" small and J is minimized. 

When the algorithm converges, 6x(i), 4w(L), and fiX(i) vanish lor all i 
and a(i) = p(i) = X(i). It can be shown that (4.31), (4.33), and (4.9) 
reduce to 

X(i) = f x T [X(.i. 4 1) - h x 1 R -1 v(i + 1)] , X (N) = 0 (4 . 35) 

w(i) » -Qf w T f; T >(i) ; x(0) = -P" 1 ^ - X„) (4.36) 

which are equivalent to (4.4) mid (4.5), the necessary conditions for a mini- 
mum performance measure . Note also that, for no process noise, the algorithm 
simplifies to a second-order parameter ident i ) icat J on method (mathematically 
equivalent to the modified Newton-Raphson method - see appendix B for details) 


V. liXAMIM.KS 


Kxumple 1 

A typical smoothing application involves r-stiintim, of first and second 
derivatives, given a noisy data record. A continuous model for this prob.cm 
is shown in Fig. 1. Note that a weighted least-squares interpretation of t-h< 
performance measure should he made in this case since the unknown input wi = 1 
be deterministic in nature. Although this application is a linear problem, it 
provides a useful check on the algorithm. 

For computer simulation, it is helpful to replace the continuous model 
with a discrete formulation given hy 


x(i + 1) = Fx(i ) + Gw(i) , x(0) = * 0 

z(i) = Hx(i) + v(i) 


(5.1) 


(5.2) 


where 




for a time step h and 


‘y(if 

= , w(i 

y (i) 


H = (] 0] 


i) ~ y(i) • 


(5.3) 


(5.4) 


The data record [z(i)) is assumed to he available for i = 1, 2, N. Ihe 

problem is to choose y(0), y(°) > nnd W U) = yO), 1 = °» I * •**» N_1, to 


minimize 


where no a 


J - ( 1 / 2 ) Z <w : ’(i)/q + 1*0 + l > - yO + D] z /R) 

l=n 

>riori knowledge of the initial conditions is given. 


(5.5) 


Implementation of the discrete algorithm (section IV) yields the Inlorma- 


tlon filter 


U- 


I 


1 


i 




M| j ( i - 1) 

M 1 2 ( i - U 


M ?2 (i " D 


aj(i - 1) 

a 2 (i - 1) 


where 


M n (i) + J/R - h'QM-^n) 

(Q/Q)N 1; . (i) + IiMj , ( i - 1) 

(Cj/q) [M 2 ;.(i) + hMijjU)] + llM 1 ;» ( i - D 
uj(i) - v ( i ) / R - liqM ]Z (i)[h« 2 (i) + w(i 
(Q/Q)|a.,(i) - hM 22 (i)w(i - 1) ] + ha, (i 

Q = Q/(l + h 2 QM 0 .. ( i ) ] . 


1)/Q] 

1) 


(5.0) 
(5. 7) 
(5.8) 
('>.')) 
(5. 10) 

(5.11) 


Equations (5.6) to (5.10) are solved "backward, that is, for 
i=N, N-l, . . . , 1, with 

Mj 1 (N) = Mi 2 (N) = M 22 (N) - Oj(N) = ot 2 (N) = 0 . 

Changes in initial conditions are then determined by solving 


(5.12) 



M n (0) 

m 12 (°)“ 

-1 

(0) 

6x_ = - 





0 

M 1? (0) 

m 2? (0) 


a 2 (0)_ 


(5.13) 

and the result is used to initialize the "forward" smoother, which is given by 
Sw(i) = -Q(w( i ) /Q + hn 0 (i + L) + hM 1; ,(i + l)[6xj(i) + h6x ? (i)] 

+ hM 22 (i + l)6x 2 (i)1 (5.14) 

6xj (i + 1) = SXjU) + li6x„ ( 1 ) (5.15) 

6x 2 (i + 1) - 5x 2 (i) + h*w(i) . 


(5. 16) 


A test case was run to simulate an aircraft descending from an altitude 
of 1100 m to touchdown in 80 sec. The initial vertical velocity was -2.5 m/sec 
Note the "true" vertical acceleration waveform as shown in Fig. 3(b). The 
data record (h = 1 sec) was obtained by integrating the acceleration twice and 
adding random noise of 10 m (rms) to simulate barometric altimeter moasu) e- 
ments. An analysis was performed with the starting sequence 
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W(i) = 0, i = 0, 1, .... N - 1, and several pairs of sLarting values for the 
Initial conditions. In each case, convergence to 

y (0) = 1101 m , y(0) -2.79 m/sec ('>.!/) 

was essentially complete after two iterations. The trajectory estimates art- 
shown in Figs. 2 and 1. 

It should be noted that the sequence [w(i)j was sensitive to the choice 
of Q and R. The results presented here were obtained with 

Q - 1.0 ; R = 100 (1.18) 

which were the mean-squared values of the [w(i)], [v(i)] sequences, respec- 
tively. An analogous problem exists in applying a "moving-arc" polynomial 
smoother (ref. 11) for estimating position, velocity, and acceleration from 
measurements of position. With that technique, a least-squares fit of a 
second-degree polynomial to n consecutive data points is "moved" through the 
data; the values of the polynomial and its derivatives at the midpoint of each 
group provide the estimation sequence. Naturally, the choice of n will 
influence the smoothed waveforms, and some experimentation may be required to 
determine a suitable value. 


Example 2 

Consider the problem of estimating the parameters y 0 and p for the 
system shown in figure /» , where both input and state measurements are cor- 
rupted by additive noise. In the estimation model, a = a(t) is a known input, 
w = w(t) is process noise, and v = v(t) is measurement noise. Here, it is 
appropriate to use a Bayesian maximum-likelihood interpretation of the per- 
formance measure (ref. 9). 

A discrete version of the model, suitable for digital-computer simula- 
tion, is given by 

y ( i + 1) - <f>y(0 + lta(i) f hw(i) , y(0) = ( r >.19) 

p(i + 1) = p(i) , |» (0) = p (*>.20) 
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where 


y-( > ) - y(i) + v(i) 


('>. 2 ! ) 


i 1 + lip . 


( r >.:>2) 


sequences (w(i)], [v(i)] are assumed to be zero-mean and white, with 
covariances Q and R, respectively. Now the problem is to choose y , p, and 
w(i) , i ™ 0, 1, N — 1, to minimj ze 

N-l 

J = (1/2) £ [w 2 (i)/Q + v 2 (i + 1)/R] (5 23 ’j 

1=0 

where, again, no ajp riori information about y Q and p is considered. 

A data record was generated with p = -1.0, y o = 0, h = 0.02, N - 300, 
and a unit-doublet input sequence, with each pulse lasting 1/4 of the record. 
The covariance of the state measurement noise was chosen to be R = 0.01, and 
values of Q/R - 0, 1, 10, 100, and 1000 were used in the simulation. 

Smoothing of the data was done by use of the discrete algorithm in section IV. 
Two analyses of each record were performed - in the first, only P and y were 
estimated, with w(i) = 0 for all i. Results for 100 Monte Carlo runs are 
shown in Table 1. The second analysis included estimation of r„(i)] (Table II). 

A comparison of the experimental results shown in Tables I and II indi- 
cates that some parameter estimates are slightly less efficient when the 
sequence [w(i)] is estimated. This behavior was reported earlier (ref. 12) 
and can be explained heuristics! 1 v. The N elements of [w(i)] are additional 
degrees of freedom in the minimization procedure, and permit better "fits" to 
the data. In some cases, however, these N elements may reduce the influence 
°f the other parameters on the performance measure. Note the apparent bias of 
the parameter estimates in Table I for large values of Q/R compared with the 
corresponding estimates in Table tl. 

By using the complete smoothing model, on,, has the distinct advantage of 
obtaining reliable predictions for parameter estimation errors as part of the 


K;k) lower hound on the error 


solution. The approximation for the Cramer- 
variance for the i th parameter estimate is given by 

<V>‘ ,r (M o l) ii • 

Note the much Improved correspondence l.etween values of o and o (rm 
value for 100 runs) given in Table It compared to those Riven in Table l 
These results, however, depend on Rood a priori knowledge of Q and K. 

VI. CONCLUDING REMARKS 


Continuous and discrete versions of a nonlinear smoothing algorithm were 
derived. Convergence characteristics were demonstrated to b; of the second 
order. Examples were provided to illustrate application of the algorithm for 
state and parameter estimation in the presence of unknown inputs that are 
deterministic and stochastic in nature. 

Mention should be made of the effort to increase the radius of conver- 
gence of the algorithm. Eliminating second-partial derivatives from lf xx , 

3C , and 3^ provides an effective approximation to the hessian matrix. For 
a system without process noise, convergence properties were shown to be 
equivalent to those obtained with the modified Newton-Raphson method. It is 
conjectured, however, that the approximation can be used In general, with good 

results . 


It is too early to specula! 
algorithm described here will 
filter-smoother procedure .. Hoe- 
ing aircraft flight-test data i - 
those obtained by use of the Hit 


whether the implementation of the smoothing 
nor, e te.Livi than existing extended Kalman 
application of the algorithm for smooth- 
anticipated, and a comparison of results with 
er-smoothvr will be included in the study. 
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APPKNDK A 


Tin-; mnk Mrmoi) and smooth jnc 

(CONTINUOUS (.ASK) 


Tin* modified Newton- T.uphstm (MNK) method (refs. 6 and 7) is widely ur.ed 
for offline parameter identification. The continuous version of t!ic algorithm 
computes a change in initial condition 

-I 

6x„ = - Ip : 1 + | S R - 1 S d-rl | P*T 1 (x . - x„) - f S T R _1 v dt | (A.l) 


x 0 = - p; 1 + f 1 s t r-‘s dr p^ (x o - 5c 0 ) ~ f f sTr “ 1v dT 

Jt o J L Jt o 


at each iteration for a system 


x - f(x) , x 


z - h(x) + v 


(A. 2) 
(A. 3) 


until the performance measure 


J • (1/2) <*„ - i 0 ) T P;‘(x 0 - 5 0 ) + (1/2) 


J"' vV>v 


dt 


(A. 4) 


is minimized. In this formulation, 

S = h x <),(t,t o ) ; 4 = f x * , f(t 0 ,t o ) = I . 


(A. 5) 


Here it is shown that the MNR method and the nonlinear smoothing algorithm 
derived in section III are equivalent for n system without process noise 
(w * 0) , and with the approximation 


T "-l, 


1f xx \ R "' h x 


(A. 6) 


First, let 


T f tf T - 

‘ J, SR 


3 R *v dt 


(A. 7) 


and differentiate both, sides to obtain 


ij)^u + a - s'r-'v . 


(A. 8) 





Hence, the MNR method is equivalent to the nonlinear smoothing algorithm pre- 
sented here for no process noise and with the aforementioned approximation of 

Tf 

xx' 


21 


APPENDIX R 

TIIF. MNP. METHOD AND SMOOTHING 
(DISCRETE CASE) 


Tho di sc rete version of the modi l ied Newton-Ruphson (MNR) method 
(refs, 6 and 7) determines the initial condition for a system modelled as 

x(i + l)~f[x(i)], x Q unknown (14.1) 

z(i + 1) - h[x(i + 1)] + v(i + 1) (B.2) 

such that a performance measure 


T q. 

J - (1/2) (x Q - x Q ) P" : (x D - x Q ) + (1/2) £ v f i + l)R _1 v(i + 1) (B.3) 

i=o 

Is minimized. In this formulation, x Q is an a priori estimate and P Q , R 
are weighting matrices. The MNR method computes a change in initial condition 
at each iteration of 


6x, 


N- 1 

- + E 

i=0 


S" (i + l)R _1 S(i + 1) 


N-l 

- £ S T (i + 1)R~ ^ v(i + 1) 
i=0 


] 


1 ^'<*0 - \ 


(B.4) 


where 


S(i + 1) = h x <Ki + 1) ; *(i + 1) = f x <J>(l) , <K0) = I (B . 5) 

and <(> ( i ) is the "sensitivity function" 

<J)(i) = ax(i)/3x 0 . (B. 6) 

It is shown here that the MNR method and the nonlinear smoothing algo- 
rithm derived in section IV are mathematically equivalent for a system without 
process noise and with the approximation 


= f.V*' V* 


O'. 7) 
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I Vi +"* •+- 


First, let 


N-l 

/(Dad) - - 2 f 4 (J + nir^a + i) 


(U. H) 


and form the difference 


* T (i)a(I) - <t> T (i + l)«(i + 1) = -S 1 ( 1 + l)K _1 v(i + 1) • 


Rearrange terms and substitute from (B.5) to obtain 


u(i) = f x f t fx ( J + 1) - h^R'^Ci + 1)1 


(B.‘J) 


(li.10) 


which Is equivalent to the relation for the costate variable given in (4.33). 
Notice that (B.8) with i = 0 becomes 


a(0) = - £ S T (j + D R_lv U + * 

j=0 


(B. 11) 


The development for the information matrix follows in similar fashion. 


<t> T (i)M(i)$(i) = j S T (J + l)R -1 S(j + 1) 


(B. 12) 


and form the difference 


* T (i)M(i)*(i) - * T <i + l)M(i + !)♦(! + 1) - S T (i + l)*->S(i + 1). (B.1D 


Rearrange terms and substitute from (B.5) to obtain 


M(i) = f x T tM(i + 1) + h x V 1 h x ]f x 


(B.14) 


which is equivalent to the expression for the information matrix given in 
(4.33). Notice that (B.13) with i =■ 0 becomes 


M(0) = T, S T (j + l)R _1 R(j + 1) 


(B . 15) 


With the substitution of (B.U) and (B.15) into (B.4), the expression for 
Sx 0 is the same as that given in (4.17). Hence, the MNR method Is equivalent 
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to the dJaerete smoothing alf»ori tlirn prenentod In flection IV, for tin- cane of 
no proccHB noise, and with the aforementioned approximation of If , 

XX 
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appknmx r 


A (.OVAKJ ANCK AI.CON ITIIM Ktll< CONTINUUM.*; SMOOTH INC 

lh,,i Ix - ™,i.« ..... 

linear smoothing Tl„. ,,lg,„HI„„ ,, 

cov.iri.im-c flltur, backward cava, I her" ,snlull,m „j ( | 

TPBVI. that r rs „J„ from „„ ,, , 

tlon III. Thera it was show,, . I,.,i approximations ,,r 0.24) „ 1 1 , 

the algorithm and extended it. radlu,, of convergence. By iMr.nla.-ln* the 
variable 


!» — A + 6X 

an equivalent linear TPBVP was determined as 


** • V* - - f„w , 6x(t„) - Sx„ 

P - -h x R *h x 6x - f x T p + h^lC'v , p(t f ) = 0 . 


(C. 1 ) 

«:. 2 ) 

(C. I) 


Another solution to (C.2) and IC.3) can be obtained using the sweep 

5x ’ S * - p l> <C./,) 

where Si . «*(») and P . P(t ). order to satisfy the boundary condition 
of (3.9) , set 


6 * (t o> “ - x G ; P(t o) = P o . 


( t: . *> ) 


Now, differentiate (C.4) and use (C.2) and (c.3) to obtain the dif . .rent ia 1 
equations 

p — • ii j nc I . ,■ .. .- T T 

(C.M 


p - f x t> + Pf x T + i P„qr T - KRK f 


Ax = f x 6x - l w w + K(v - ii x Ax) 


(C. 7) 


where 


K - Ph 'ir 1 . 


(C.8) 


Equations (C.6) and (,:.7) cuuu , I uto „ forward covariance filter. 
is, their solutions are propagated forward from 1 - t)1 until I ■, , I,. 
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cnmpleio the solution of the Tl’NVT, siibst I lute (C.4) into (C.3) and oblaln tlx* 
backward smoother 

(* = ~(f x - db x ) + h x 1 It ' (v - h x dx) , r*(tj.) = 0 . (C.9) 

While (C.9) Is being propagated backward from t — t ^ until t = i (1 . 
the updated value of the unknown forcing function can be determined from 

w = -Qf w *p . (C.10) 

Equation (C,10) follows directly from (3.10), (3.28), and (C.l). At the con- 
clusion of the backward pass, the updated initial condition can be determined 
from 

x o * *o " Vo (C * U) 

where p Q * p(t Q ). This relation is obtained from (C.4) and (C.5). 

The steps in the algorithm can now be summarized as follows: 

(1) Use x Q ,w(t) obtained from the preceding iteration (or an initial 
guess) to compute a smoothed trajectory x(t) from (2.1) and the performance 
measure from (2.3). 

(2) Solve the "forward covariance filter" (C.6) and (C.7) to obtain 
K(t) and dx(t). Store the elements necessary for the next step. 

(3) Solve the "backward covariance smoother" (C.9) and evaluate w(t) 
from (C.10). Determine the updated initial condition x Q from (C.ll). 

(4) Iterate until the performance measure has reached a minimum. 

The algorithm presented here is, in effect, a linear Kdlman filter- 
smoother. It has the advantage of not requiring a matrix inversion to deter- 
mine 6x q . However, it may be difficult to "start" the filter In the absence 
of a jrriori information. No such difficulty will be experienced using the 
algorithm of section III. 
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Notice that when the algorithm converges, 6x(t), <Sw(t), and 6>.(l) vanish, 
so that p(t) ■* X(t). It Is easy to show that (C.3), (C.10), and (C.1I) are 
equivalent to (3.4) and (3.5), Lho conditions necessary for a minimum perfor- 
mance measure. 

For the linear system described by (3.37) to (3.39), the covariance 
algorithm a Lso converges in one iteration. To demonstrate this fact, make 
a change of variable 


fix - x - x n ; fix a x - x^ . 


(C. 12) 


The resulting covariance filter-smoother equations are given by 


P = FP + PF T + TQI ,T - KRK ; 


k = Fx + Gu + K[z - (Hx + Du)]; 


(C. 13) 


where K = PhV 1 , P(t Q ) = P Q , x(t 0 ) - x Q , and 


£ * -(F - KH) T p + H T R - 1 [ z - (Hx + Du)]; 


w = -Qr p 


(C. 14) 


with p(t f ) = 0. At the conclusion of the backward pass, the initial con- 
dition is obtained from 


x o x o “ ^o p o 


(C* 15) 



APPENDIX I) 


A COVARIANCE ALGORITHM FOR DISCRETE SMOOTHING 


In this appendix, a discrete "forward covariance filter," "backward 
covariance smoother" algorithm for nonlinear smoothing is derived. The deriva- 
tion will be done using the approximations of (4.24) to simplify the accessory 
minimization problem and extend the convergence interval. The development of 
the discrete algorithm is similar to that presented in appendix C for the con- 
tinuous case. Recall that in section IV, introduction of the variable 


P (i) = X(l) + SA(i) 
led to the equivalent linear TPBVP 


(D.l) 


6x(i + 1) - f x 6x(i) - f w Qf w T f; T p(i) - f w w(i) , «*(0) = «x 0 


p(i) = f v T {p(i + 1) - h T R~ 1 [ v ( i + 1) - h x 5x(i +1)]} , p(N) = 0 


(D. 2) 
(D.3) 


To solve (D.2) and (D.3), the sweep 

6x(i) - <5x(i) - P(i)p (i) 

will be used. The boundary condition of (4.9) will be satisfied if 


(D.4) 


6x(0) - x D - x 0 ; P (0) - P c 


(n.5) 


Now, use (D.4) in (D.2) and (D.3) to effect a separation of solutions. After 
some algebraic manipulation, the equations for the forward covariance filter 
are obtained in the form of a time update: 


6x(i + 1) - f x 5x(i) - f w w(i) 5 


M(i + 1) = f v P(i)f x + f w Qf w 


I 


(D .6) 


and a measurement update: 

6x(i + 1) = 5x(i ■<- 1) + K(i + 1) fv(i + 1) - h x 6x(l + 1) ] ! 


K(i + 1) = M(i + l)h T [R + h M(i + Dh x T ] _1 ; 


(D. 7) 


P(i +!)=[!- K(i + l)h ]M(1 + 1) . 
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I 


solution of the TPHVP fr w 

advantageous to make a change of variable ’ “ ^ 

P(i) = f x T fi (i + 1) , 

Substitute (D.4) and (0.8) into (D , 

( .3) and make use of the fact that 


K(i) = P(i) h x T R'l 


to express the backward covariance smoother as 

B(1) " 11 ' K(1)h * )T ’ ( ‘> - ‘s.V'ivU) - h> (1)] , #00 . 0 ,, 

t(1 - 1) - f/sCX) . ■ (; 

During the backward pass, updated values of the for < , 

be calculated from Cing functi °n estimate 


w(i - 1) = -Qf w T g(i) . 


Finally, the Initial condition for the nest iteration is given by 

x o - S o - ty«» • 

Equation (D.U) is obtained using (4.24) (4 271 „ „ 

«.10), while CD. 12, follow from (D.4, J ‘ <D - 10> “ Uh 

The steps In the algorithm are sum^rited as follows, 

initial guess, “ 

value of J f rom (2,6). * ^ f ° m and the 

(2) Solve the "forward covariance filter" fn M , , 

[K(i)] and [<5x(i)1 Stor^ i * ant ^ to obtain 

St ° re elementH necessary f or the next step. 

(3) Solve the "backward covariance smoother" ( D 10) nn H i 

from ~ — condition"^ t:z:z Mi 

Itert.e until the performance measure J ia minimized 
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